#include <CL/opencl.h>
#include <math.h>
#include <string.h>

#include "PGLTypes.h"

// Default constructor
PGL::pglVector::pglVector()
{
	vec.s[0] = 0;
	vec.s[1] = 0;
	vec.s[2] = 0;
	vec.s[3] = 1;
}

// Copy constructor
PGL::pglVector::pglVector(const pglVector &other)
{
	vec.s[0] = other.vec.s[0];
	vec.s[1] = other.vec.s[1];
	vec.s[2] = other.vec.s[2];
	vec.s[3] = other.vec.s[3];
}

// Construct with x,y,z
PGL::pglVector::pglVector(cl_float x, cl_float y, cl_float z)
{
	vec.s[0] = x;
	vec.s[1] = y;
	vec.s[2] = z;
	vec.s[3] = 1;
}

// Construct with x,y,z,w
PGL::pglVector::pglVector(cl_float x, cl_float y, cl_float z, cl_float w)
{
	vec.s[0] = x;
	vec.s[1] = y;
	vec.s[2] = z;
	vec.s[3] = w;
}

// Random access (non-const)
cl_float& PGL::pglVector::operator[]( unsigned int i )
{
	return vec.s[i];
}

// Assignment
PGL::pglVector& PGL::pglVector::operator=(const pglVector &other)
{
	pglVector(other.x(),other.y(),other.z());
	return *this;
}



// Vector addition
PGL::pglVector PGL::pglVector::operator+(const pglVector &other) const
{
	return pglVector(x()+other.x(),y()+other.y(),z()+other.z());
}

// Vector increment
PGL::pglVector& PGL::pglVector::operator+=(const pglVector &other)
{
	pglVector(x()+other.x(),y()+other.y(),z()+other.z());
	return *this;
}

// Random access (const)
const cl_float PGL::pglVector::operator[]( unsigned int i ) const
{
	return vec.s[i];
}

// Vector subtraction
PGL::pglVector PGL::pglVector::operator-(const pglVector &other) const
{
	return pglVector(x()-other.x(),y()-other.y(),z()-other.z());
}

// Negative vector
PGL::pglVector PGL::pglVector::operator-(void) const
{
	return pglVector(-x(),-y(),-z());
}

// Scalar multiplication
PGL::pglVector PGL::pglVector::operator*(const cl_float &scalar) const
{
	return pglVector(x()*scalar,y()*scalar,z()*scalar);
}

// Scalar division
PGL::pglVector PGL::pglVector::operator/(const cl_float &scalar) const 
{
	cl_float rcpScalar = 1.0f/scalar;
	return (*this)*rcpScalar;
}

// * with 2 vectors means dot product
cl_float PGL::pglVector::operator*(const pglVector &other) const 
{
	cl_float dotProduct = 0;
	for(int i=0; i<4; i++)
		dotProduct += vec.s[i]*other[i];
	return dotProduct;
}

// Vector matrix multiplication
PGL::pglVector PGL::pglVector::operator*(const pglMatrix &mat) const 
{
	pglVector product;

	for(int i=0; i<4; i++)
	{
		product[i] = (*this)*pglVector(mat[0][i],mat[1][i],mat[2][i],mat[3][i]);
	}

	return product;
}

// ^ with 2 vectors means cross product
PGL::pglVector PGL::pglVector::operator^(const pglVector &other) const 
{
	pglVector crossProduct;

	crossProduct.vec.s[0] = vec.s[1]*other.vec.s[2] - vec.s[2]*other.vec.s[1];
	crossProduct.vec.s[1] = vec.s[2]*other.vec.s[0] - vec.s[0]*other.vec.s[2];
	crossProduct.vec.s[2] = vec.s[0]*other.vec.s[1] - vec.s[1]*other.vec.s[0];

	return crossProduct;
}

// Vector magnitude
cl_float PGL::pglVector::magnitude() const 
{
	return sqrt(x()*x()+y()*y()+z()*z());
}

// Normalize vector
PGL::pglVector PGL::pglVector::normalize() const 
{
	cl_float mag = magnitude();
	return (*this)/mag;
}

// Print operator
std::ostream& PGL::operator<<( std::ostream &out, pglVector &pglVec )
{
	out << "< " << pglVec.x() << " , " << pglVec.y() << " , " << pglVec.z() << " , " << pglVec.w() << " >";
	return out;
}

// pglNormalVertex default constructor
PGL::pglNormalVertex::pglNormalVertex()
{

}

// pglNormalVertex constructor
PGL::pglNormalVertex::pglNormalVertex(pglVector norm, pglVector pos)
	:normal(norm),position(pos)
{

}

// pglMatrix default constructor
PGL::pglMatrix::pglMatrix()
{
	memcpy(r,pglIdentityMatrix,sizeof(pglMatrix));
}

PGL::pglMatrix::pglMatrix(const pglVector row0, const pglVector row1, const pglVector row2, const pglVector row3)
{
	r[0] = row0;
	r[1] = row1;
	r[2] = row2;
	r[3] = row3;
}

// Assign matrix
PGL::pglMatrix& PGL::pglMatrix::operator=(const pglMatrix &other)
{
	pglMatrix(other[0],other[1],other[2],other[3]);
	return *this;
}

// Matrix random access
PGL::pglVector& PGL::pglMatrix::operator[]( unsigned int i )
{
	return r[i];
}

// Matrix const random access
const PGL::pglVector PGL::pglMatrix::operator[]( unsigned int i ) const
{
	return r[i];
}

// Matrix multiplication
PGL::pglMatrix PGL::pglMatrix::operator*( const pglMatrix &other ) const
{
	// Naif!
	pglMatrix out;

	for(int i=0; i<4; i++)
	{
		for (int j=0; j<4; j++)
		{
			out[i][j] = 0;

			for(int x=0; x<4; x++)
			{
				out[i][j] += r[i][x]*other[x][j];
			}
		}
	}

	return out;
}
// *********************************************** //
// Matrix functions
// *********************************************** //

void PGL::matrixLoadIdentity(pglMatrix* out)
{
	*out = pglMatrix();
}

void PGL::matrixLoadTranslationToXYZ( pglMatrix* out,  pglVector trans )
{
	(*out) = pglMatrix();

	(*out)[3][0] = trans[0];
	(*out)[3][1] = trans[1];
	(*out)[3][2] = trans[2];
}


void PGL::matrixLoadRotationAboutX( pglMatrix* out,  cl_float rotation )
{
	(*out) = pglMatrix();

	(*out)[1][1] = +cos(rotation);
	(*out)[2][1] = -sin(rotation);
	(*out)[1][2] = +sin(rotation);
	(*out)[2][2] = +cos(rotation);
}

void PGL::matrixLoadRotationAboutY( pglMatrix* out,  cl_float rotation )
{
	(*out) = pglMatrix();

	(*out)[0][0] = +cos(rotation);
	(*out)[2][0] = +sin(rotation);
	(*out)[0][2] = -sin(rotation);
	(*out)[2][2] = +cos(rotation);
}

void PGL::matrixLoadRotationAboutZ( pglMatrix* out,  cl_float rotation )
{
	(*out) = pglMatrix();

	(*out)[0][0] = +cos(rotation);
	(*out)[1][0] = -sin(rotation);
	(*out)[0][1] = +sin(rotation);
	(*out)[1][1] = +cos(rotation);
}

void PGL::matrixLoadRotationAboutAxis( pglMatrix* out,  cl_float rotation, pglVector axis )
{
	(*out) = pglMatrix();

	pglVector normalizedAxis = axis.normalize();

	cl_float cosR = cos(rotation);
	cl_float sinR = sin(rotation);
	cl_float oneMinusCosR = 1.0f - cosR;
	
	(*out)[0][0] = (axis[0]*axis[0]) * oneMinusCosR + cosR;
	(*out)[0][1] = (axis[0]*axis[1]) * oneMinusCosR + axis[2] * sinR;
	(*out)[0][2] = (axis[0]*axis[2]) * oneMinusCosR - axis[1] * sinR;

	(*out)[1][0] = (axis[1]*axis[0]) * oneMinusCosR - axis[2] * sinR;
	(*out)[1][1] = (axis[1]*axis[1]) * oneMinusCosR + cosR;
	(*out)[1][2] = (axis[1]*axis[2]) * oneMinusCosR + axis[0] * sinR;

	(*out)[2][0] = (axis[2]*axis[0]) * oneMinusCosR + axis[1] * sinR;
	(*out)[2][1] = (axis[2]*axis[1]) * oneMinusCosR - axis[0] * sinR;
	(*out)[2][2] = (axis[2]*axis[2]) * oneMinusCosR + cosR;
}

void PGL::matrixLoadLookAt( pglMatrix* out,  pglVector eye, pglVector at, pglVector up )
{
	(*out) = pglMatrix();

	eye[3] = 0;
	at[3] = 0;
	up[3] = 0;

	pglVector zaxis = (at-eye).normalize();
	pglVector xaxis = (up^zaxis).normalize();
	pglVector yaxis = zaxis^xaxis;

	(*out)[0][0] = xaxis.x();
	(*out)[1][0] = xaxis.y();
	(*out)[2][0] = xaxis.z();
	(*out)[3][0] = -(xaxis*eye);

	(*out)[0][1] = yaxis.x();
	(*out)[1][1] = yaxis.y();
	(*out)[2][1] = yaxis.z();
	(*out)[3][1] = -(yaxis*eye);

	(*out)[0][2] = zaxis.x();
	(*out)[1][2] = zaxis.y();
	(*out)[2][2] = zaxis.z();
	(*out)[3][2] = -(zaxis*eye);
}

void PGL::matrixLoadOrthographicProjection( pglMatrix* out,  cl_float right, cl_float left, cl_float top, cl_float bottom, cl_float near, cl_float far )
{
	(*out) = pglMatrix();

	(*out)[0][0] = 2.0f / (right-left);
	(*out)[1][1] = 2.0f / (top-bottom);
	(*out)[2][2] = -2.0f / (far-near);
	(*out)[3][3] = 1;

	(*out)[3][0] = - ( (right+left) / (right-left) );
	(*out)[3][1] = - ( (top+bottom) / (top-bottom) );
	(*out)[3][2] = - ( (far+near) / (far-near) );
}

void PGL::matrixLoadPerspectiveProjection( pglMatrix* out,  cl_float fovy, cl_float aspect, cl_float zNear, cl_float zFar )
{
	cl_float yScale = 1.0f/tan(fovy/2.0f);
	cl_float xScale = yScale / aspect;

	(*out)[0][0] = xScale;
	(*out)[1][1] = yScale;
	(*out)[2][2] = (zFar) / (zFar-zNear);
	(*out)[2][3] = 1;
	(*out)[3][2] = -(zNear*zFar) / (zFar-zNear);
	(*out)[3][3] = 0;
}

void PGL::matrixLoadViewportTransform( pglMatrix* out, cl_float x, cl_float y, cl_float width, cl_float height, cl_float zNear, cl_float zFar )
{
	(*out) = pglMatrix();

	(*out)[0][0] = width/2.0f;
	(*out)[1][1] = -height/2.0f;
	(*out)[2][2] = (zFar-zNear);
	(*out)[3][0] = x + (*out)[0][0];
	(*out)[3][1] = y - (*out)[1][1];
	(*out)[3][2] = zNear;
}
